import sim
import time
import numpy as np
import math

# 连接到CoppeliaSim
clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 5)

if clientID != -1:
    print('Connected to CoppeliaSim')

    # 获取机器人的虚拟链接句柄
    joint_names = ['joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6']  
    joint_handles = []
    for joint_name in joint_names:
        res, joint_handle = sim.simxGetObjectHandle(clientID, joint_name, sim.simx_opmode_blocking)
        if res == sim.simx_return_ok:
            joint_handles.append(joint_handle)
            print(f'Joint handle for {joint_name}: {joint_handle}')
        else:
            print(f'Failed to get joint handle for {joint_name}')

    if len(joint_handles) == len(joint_names):
        # 设置目标角度，传参入口，角度要保证为弧度制
        target_angles = [25 * math.pi / 180, 20* math.pi / 180, 100 * math.pi / 180, 20 * math.pi / 180, 85 * math.pi / 180, 0]

        # 分步运动循环
        num_steps = 5  # 分步运动的步数
        for step in range(num_steps):
            # 计算每个关节的目标角度（初始值为空），当传入下一个参数之前，机械臂会保持上次设置的角度
            current_angles = []
            for i, joint_handle in enumerate(joint_handles):
                res, joint_angle = sim.simxGetJointPosition(clientID, joint_handle, sim.simx_opmode_blocking)
                if res == sim.simx_return_ok:
                    current_angles.append(joint_angle)
                else:
                    print(f'Failed to get current angle for joint {joint_names[i]}')
                    break

            # 插值计算每个步骤的目标角度
            step_angles = []
            for i in range(len(target_angles)):
                diff = target_angles[i] - current_angles[i]
                step_angle = current_angles[i] + (diff / num_steps) * (step + 1)
                step_angles.append(step_angle)

            # 设置关节目标角度
            for i, joint_handle in enumerate(joint_handles):
                res = sim.simxSetJointTargetPosition(clientID, joint_handle, step_angles[i], sim.simx_opmode_blocking)
                

        # 暂停一小段时间，模拟仿真的过程
        time.sleep(0.1)

        # 关闭连接
        sim.simxFinish(clientID)
        print('Connection closed')

else:
    print('Failed to connect to CoppeliaSim')
